/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package ticks.chromedome.subsystems;

import Threads.VisionProcessing;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import ticks.chromedome.Robot;
import ticks.chromedome.commands.CommandBase;

/**
 *
 * @author Jared
 */
public class VisionPID extends PIDSubsystem {

    private static final double Kp = 1.0;
    private static final double Ki = 0.0;
    private static final double Kd = 0.0;
     

    // Initialize your subsystem here
    public VisionPID() {
        super("VisionPID", Kp, Ki, Kd);
        enable();
        setSetpoint(0.0);
        // Use these to get going:
        // setSetpoint() -  Sets where the PID controller should move the system
        //                  to
        // enable() - Enables the PID controller.
    }
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    
    
    protected double returnPIDInput() {
        // Return your input value for the PID loop
        // e.g. a sensor, like a potentiometer:
        // yourPot.getAverageVoltage() / kYourMaxVoltage;
        return VisionProcessing.getInstance().getTargetXVal();
    }
    
    protected void usePIDOutput(double output) {
        if(CommandBase.robot_global.visionEnabled()==true){
            Turret.getInstance().setTurretMotorSpeed(output);
        }
    }
}
